#include "ShooterAngleControl.h"
#include "../Robotmap.h"

ShooterAngleControl::ShooterAngleControl(float SetPoint) {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	Requires(shooterAngle);
	SetToPoint = SetPoint;
}

// Called just before this Command runs the first time
void ShooterAngleControl::Initialize() {
	
}

// Called repeatedly when this Command is scheduled to run
void ShooterAngleControl::Execute() {
	//	float RecievedAngle;
	//	if((RecievedAngle - shooterAngle->GetCurrentShooterAngle()) >= 0 && fabs(shooterAngle->GetCurrentShooterAngle() - RecievedAngle) >= 2){
	//		shooterAngle->SetShooterAngleDirection(1);
	//	} else if((RecievedAngle - shooterAngle->GetCurrentShooterAngle()) <= 0  && fabs(shooterAngle->GetCurrentShooterAngle() - RecievedAngle) >= 2){
	//		shooterAngle->SetShooterAngleDirection(-1);
	//	}
	//	shooterAngle->GetCurrentShooterAngle();
	//	shooterAngle->SetSetpoint(SetToPoint);
	// TODO: Make this work better for certain sitamawations
	/*if (client->dataAngle > 8 && client->dataAngle < 35) {
		shooterAngle->SetSetpoint(client->dataAngle);
		shooterAngle->Enable();
			
	} else {
		shooterAngle->Disable();
	}*/
	if(oi->GetShooterOverride()){
		//shooterAngle->SetSetpointRelative(0.0);
	}else{
	DSPrintLine(LINE2, "SA: %.2f", shooterAngle->GetSetpoint());
	latch = oi->shooterJoystick.GetRawButton(8);
	if (latch && !oldlatch) {
		if (client->dataAngle < 100.0) {
			shooterAngle->SetSetpoint(client->dataAngle);
			shooterAngle->Enable();
			
		} else {
			shooterAngle->SetSetpointRelative(0.0);
	}
	}
	oldlatch = latch;
	}
	shooterAngle->GetCurrentShooterAngle();

//	shooterAngle->SetSetpoint(client->dataAngle);
	
	DSPrintLine(LINE5, "Angle: %.2f", client->dataAngle);
	DSPrintLine(LINE6, "P %.2f I %.2f D %.2f", client->P, client->I, client->D);
}
// Make this return true when this Command no longer needs to run execute()
bool ShooterAngleControl::IsFinished() {
	//	float RecievedAngle;
	//	if (fabs(shooterAngle->GetCurrentShooterAngle() - RecievedAngle) <= 2){
	//		return true;
	//	} else {
	//		return false;
	//	}
	//	return shooterAngle->OnTarget();
	return false;
}

// Called once after isFinished returns true
void ShooterAngleControl::End() {
	//	shooterAngle->SetShooterAngleDirection(0);
	shooterAngle->Disable();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
void ShooterAngleControl::Interrupted() {
	//	shooterAngle->SetShooterAngleDirection(0);
	shooterAngle->Disable();
}
